#include "Task.h"
#include "stm32f4xx.h"
#include "GlobalData.h"
#include "CAN.h"
#include "timer.h"
#include "PID.h"
#include "motor.h"
#include "Sick_V2.h"
#include "commModule/protocol_upmotor.h"
#include "commModule/protocol_grapmotor.h"
#include "commData.h"
#include "gpio.h"
#include "stdio.h"
#include "usart.h"

#define FILTER_N 6
float filter_buf[FILTER_N + 1];
float sickFilter(float data)
{
    int8_t i;
    //float filter_sum = 0;
    static float aver_val = 0;
    static float aver_old = 0;
    static int num_rec = 0;
    float offset = 0;
    
    for(i = 0; i < 5; i++)
    {
        filter_buf[i] = filter_buf[i+1];
    }
    filter_buf[5] = data;
    
    
    //脗脣碌么鹿媒麓贸碌脛脢媒脰碌
    offset = (aver_old>data)?(aver_old-data):(data-aver_old);
    if(offset>200){
        num_rec = 1;
        aver_old = data;
    }else{
        num_rec++;
        if(num_rec>5){
            num_rec = 6;
            offset = (aver_old>aver_val)?(aver_old-aver_val):(aver_val-aver_old);
            if(aver_old>aver_val){
                if(offset>200){
                    aver_val += 200;
                }else{
                    aver_val = data;
                } 
            }else{
                if(offset>200){
                    aver_val -= 200;
                }else{
                    aver_val = data;
                } 
            }           
        }
    }
    return aver_val;
//    if((data - aver_old) > 200)
//        filter_buf[FILTER_N] = aver_old+100;
//    else
//        filter_buf[FILTER_N] = data;
//    
//    for(i = 0; i < FILTER_N; i++)
//    {
//        filter_buf[i] = filter_buf[i + 1];
//        filter_sum += filter_buf[i];
//    }
//    aver_val = (filter_sum / FILTER_N);
//    aver_old = aver_val;
//    return aver_val;
}

float filter_buf2[FILTER_N + 1];
float sickFilter2(float data)
{
    int8_t i;
    float filter_sum2 = 0;
    static float aver_val2 = 0;
    static float aver_old2 = 0;
    
    //脗脣碌么鹿媒麓贸碌脛脢媒脰碌
//    if((data - aver_old2) > 200)
//        filter_buf[FILTER_N] = aver_old2;
//    else
//        filter_buf[FILTER_N] = data;
//    
    for(i = 0; i < FILTER_N; i++)
    {
        filter_buf[i] = filter_buf2[i + 1];
        filter_sum2 += filter_buf2[i];
    }
    aver_val2 = (filter_sum2 / FILTER_N);
    aver_old2 = aver_val2;
    return aver_val2;
}

uint8_t GetFlowerPosi(void)
{
    uint8_t i;
    uint16_t x,y;
    int16_t dd, dd_old;
    
    for (i = 0; i < Sick_Result.size; i++)
	{
        if (Sick_Result.Queue[i].circ_r2 >= 100 - 20 && Sick_Result.Queue[i].circ_r2 <= 100 + 20)  /* 赂脙虏脦脢媒脨猫脪陋卤锚露篓 */
		{
            
            x = Sick_Result.Queue[i].circ_x;
            y = Sick_Result.Queue[i].circ_y;
            dd = -(int16_t)((x - y - 345) * 0.7);
            OutData[0] = sickFilter(dd);
            dd_old = OutData[0];
        }
        else
        {
//            OutData[0] = Filter(dd_old);
//            dd_old = OutData[0];
        }
    }
    return 0;
}


int16_t flagdd;
uint16_t threValue = 300;
int16_t posiX[5];
int16_t posiY[5];

/***********************************************************************************************
* @Function  : SearchFlowerFunc
*
* @Breif     : 脩掳脮脪禄篓脜猫拢卢鲁碌脡铆虏脦驴录脰卤脧脽路陆鲁脤 y = x - 332 (mm)
*
* @Param[in] : None
*
* @Return    : None
***********************************************************************************************/
Sick_Queue_small  G_ResultT;
Sick_Info   G_InfoT;
void SearchFlowerFunc(void)
{
    uint8_t i;
    int16_t x, y;      /* 禄篓脜猫脰脨脨脛脳酶卤锚 */
    static float dd;
    static uint8_t stopCount = 0;
    static uint8_t transCount = 0;
    static uint8_t stopFlag = 0;
    static uint8_t transFlag = 0;
    static float dd_old;
    float dist;
    int max_bias = 0;
    int max_index = 0;
    uint16_t temp;
    OutData[1] = runTime * 1.0;
    runTime = 0;
    G_ResultT.size = 0;
    float thre_dis;

    //1.
	for (i = 0; i < Sick_Result.size; i++)
	{
		if(G_ResultT.size>10)break;
        if (Sick_Result.Queue[i].circ_r2 >= (100 - 40) && Sick_Result.Queue[i].circ_r2 <= (100 + 40))  /* 赂脙虏脦脢媒脨猫脪陋卤锚露篓 */
		{
            G_ResultT.Queue[G_ResultT.size] = Sick_Result.Queue[i];
            G_ResultT.size++;        
        }
    }
    //2.
	for (i = 0; i < G_ResultT.size; i++)
	{
		if (max_bias<=G_ResultT.Queue[i].circ_r2_bias)  
		{
            max_bias = G_ResultT.Queue[i].circ_r2_bias;
            max_index = i;
        }
    }
    //3.
    G_InfoT = G_ResultT.Queue[max_index];
    //4.
    /* 禄篓脜猫脰脨脨脛脳酶卤锚 */
    x = G_InfoT.circ_x;
    y = G_InfoT.circ_y;
    //x = sickFilter(x);
    //y = sickFilter2(y);
    
    //OutData[1] = x;
    //OutData[2] = y;
    //OutData[3] = Sick_Result.Queue[i].circ_r2_bias;
    dd_old = dd;
    dd = -((x - y - 325) * 0.7);
    dd = sickFilter(dd) + 15;//20
    
    OutData[0] = dd;
    /* 脛拢脢陆脟脨禄禄 */
    dist = (x + y) * 0.7;
    //dist = sickFilter2(dist);
    //OutData[3] = dist;
   
    
    // 潞谩脧貌戮脿脌毛
    if (dd < 35 && dd > -35)
    {
        stopCount ++;
    }
    else
    {
        if (stopCount > 0)
        {
            stopCount--;
        }
        if(stopCount < 5)
        {
            transFlag = 0;
        }
    }
    if(stopCount >= 10)
    {
        stopCount = 10;
        transFlag = 1;
    }
    if(transFlag == 1)
    {
        leftMotorSpeed.rpm = motorSpeedPreSet;
        rightMotorSpeed.rpm = motorSpeedPreSet;// + 50;
        
        if(runMode == SEARCHFLOWER)//400，寻花转搬花，700，巡线转摆花
        {
            thre_dis = 400;
        }
        else if(runMode == LEAD)
        {
            thre_dis = 500;
        }
        
        // 脳脻脧貌戮脿脌毛
        if(dist <= thre_dis && dist != 0)
        {
            transCount++;
        }
        else
        {
            if(transCount > 0)
            {
                transCount --;
            }
            if(transCount < 5)
            {
                stopFlag = 0;
            }
        }
        if(transCount >= 10)
        {
            stopFlag = 1;
        }
    }
    else
    {
        controlOutValue = dd * 10;
        /*
        ** 脡猫露篓碌莽禄煤脳陋脣脵潞脥脳陋露炉路陆脧貌
        */
        
        if (controlOutValue >= 0)
        {
            temp = controlOutValue;
            leftMotorSpeed.rpm = motorSpeedPreSet;
            if(temp < threValue)
            {
                rightMotorSpeed.rpm = motorSpeedPreSet + temp;
            }
            else
            {
                rightMotorSpeed.rpm = motorSpeedPreSet + threValue;
            }
        }
        else if (controlOutValue < 0)
        {
            temp = (0 - controlOutValue);
            rightMotorSpeed.rpm = motorSpeedPreSet;
            if(temp < threValue)
            {
                leftMotorSpeed.rpm = motorSpeedPreSet + temp;
            }
            else
            {
                leftMotorSpeed.rpm = motorSpeedPreSet + threValue;
            }
        }
    }
    //修复开机自动右转的bug
    if(filter_buf[0] == 227.5 && filter_buf[5] == 227.5)/* && 
        (filter_buf[1] == filter_buf[2]) && 
        (filter_buf[2] == filter_buf[3]) &&
        (filter_buf[3] == filter_buf[4]) )*/
    {
        rightMotorSpeed.rpm = 0;
        leftMotorSpeed.rpm = 0;
    }
    
    MotorDriverTask();
    if((stopFlag == 1) && (transFlag != 0))
    {
        if(runMode == SEARCHFLOWER)
        {
            runMode = CARRY;
        }
        else if(runMode == LEAD)//巡线模式转摆花模式时使用
        {
            RobotRotationControl(300, 1, 300, 1);
            rightMotorSpeed.rpm = 600;
            leftMotorSpeed.rpm = 600;
            MotorDriverTask();
            wait_for_timeout(800);
            rightMotorSpeed.rpm = 0;
            leftMotorSpeed.rpm = 0;
            MotorDriverTask();
            runMode = PLACE;
        }
    }
}

/*************************************************************************************
* @Function  : CarryFlowerFunc
*
* @Breif     : 掳谩禄篓脛拢脢陆脰麓脨脨潞炉脢媒隆拢
*
* @Param[in] : None
*
* @Return    : None
*************************************************************************************/
uint8_t CarryFlowerFunc(void)
{
    static uint8_t size_last = 0;
	uint8_t len, i,firstPos_Volt;
    uint8_t Num_Monitor = 0;
    
    leftMotorSpeed.rpm = 0;
    rightMotorSpeed.rpm = 0;
    leftMotorSpeed.dir = 1;
    rightMotorSpeed.dir = 1;
    MotorDriverTask();

    //Init position
    /*upmotor setting,first position find*/
    if(0!=upmotor_firstPosition_Set()){
        //while(1){
            wait_for_timeout(500);
            firstPos_Volt = GPIO_ReadInputDataBit(JOY_A_KEY_Port,JOY_A_KEY_Pin);
            //usart1Printf("upmotor position set error, please reset~%d", firstPos_Volt);
        //}
    }
    //usart1Printf("upmotor position set ok, please reset~");
    wait_for_timeout(100);

    if(0!=grapmotor_firstPosition_Set()){
        //while(1){
            wait_for_timeout(50);
            firstPos_Volt = GPIO_ReadInputDataBit(JOY_A_KEY_Port,JOY_A_KEY_Pin);
            //usart1Printf("grapmotor position set error, please reset%d~",firstPos_Volt);
        //}
    }
    wait_for_timeout(50);
    //usart1Printf("grapmotor first position set ok, please reset~");
    wait_for_timeout(50);
    //usart1Printf("grapmotor grap set, please wait~");    

    if(1==Ingrap_Action_down(320)){
        for(;;){
            wait_for_timeout(100);
            //usart1Printf("upmotor down set ok, please reset~");
        }
    }
    /*grapmotor setting, first position find*/
    //usart1Printf("grapmotor first position find~");
    wait_for_timeout(50);

    /* 前进一段距离 */
    rightMotorSpeed.rpm = 500;
    leftMotorSpeed.rpm = 500;
    MotorDriverTask();
    wait_for_timeout(200);
    rightMotorSpeed.rpm = 0;
    leftMotorSpeed.rpm = 0;
    MotorDriverTask();

    if(1==Ingrap_Action_grap_v2()){
        //while(1){
            wait_for_timeout(100);
            firstPos_Volt = GPIO_ReadInputDataBit(JOY_A_KEY_Port,JOY_A_KEY_Pin);
            //usart1Printf("grapmotor grap set error, please reset%d~",firstPos_Volt);
        //}
    }
//    rightMotorSpeed.rpm = 300;
//    leftMotorSpeed.rpm = 0;
//    MotorDriverTask();
//    wait_for_timeout(100);
//    rightMotorSpeed.rpm = 0;
//    leftMotorSpeed.rpm = 300;
//    MotorDriverTask();
//    //usart1Printf("grapmotor grap set ok~");
//    wait_for_timeout(100);
//    rightMotorSpeed.rpm = 0;
//    leftMotorSpeed.rpm = 0;
//    MotorDriverTask();
    wait_for_timeout(50);
    //usart1Printf("upmotor up set, please wait~");
    if(1==Ingrap_Action_Up(300)){
        for(;;){
            wait_for_timeout(100);
            //usart1Printf("upmotor up set ok, please reset~");
        }
    }
    //usart1Printf("loop test end~");
    runMode = SEARCHLINE;
    wait_for_timeout(100);
    //for(;;);
    return 0;
}


uint8_t rotationCount = 0;
/*
 * 脩掳脧脽脠脦脦帽
 */
void SearchLineFunc(void)
{
    uint8_t robotPos;
    int8_t posErr;

    
    /* 禄帽脠隆碌卤脟掳碌脛脪媒碌录脧脽脦禄脰脙脨脜脧垄 */
    leadLinePosi.poFL = CANRxData[CAMERA_INDEX_FRONT_LEFT].rxdata[2];
    leadLinePosi2.poFL = CANRxData[CAMERA_INDEX_FRONT_LEFT].rxdata[3] + 20;
    //OutData[0] = leadLinePosi.poFL;
    //OutData[1] = leadLinePosi2.poFL;
    if(leadLinePosi2.poFL > 0 && leadLinePosi.poFL > 0)
    {
        //robotPos = (leadLinePosi.poFL + leadLinePosi2.poFL) / 2;
        posErr = leadLinePosi.poFL - leadLinePosi2.poFL;
    }
    else
    {
        posErr = 0;
    }
    
    if(posErr >= 10)//左转调节
    {
        rightMotorSpeed.rpm = 0;
        leftMotorSpeed.rpm = 300;
    }
    else if (posErr <= -10)//左转调节
    {
        rightMotorSpeed.rpm = 300;
        leftMotorSpeed.rpm = 0;
    }
    else
    {
        rightMotorSpeed.rpm = 800;
        leftMotorSpeed.rpm = 800;
    }
    MotorDriverTask();

    /*
    ** 碌卤录矛虏芒碌陆脪媒碌录脧脽脭貌脰麓脨脨脳陋脥盲露炉脳梅拢卢脳陋脧貌脪媒碌录脛拢脢陆
    */
    if (leadLinePosi.poFL >= 160 && leadLinePosi2.poFL >= 160)
    {
        rotationCount ++;
        /*
        ** 1000rpm 碌脛脣脵露脠拢卢脧貌脫脪拢篓1拢漏脳陋 90 隆茫
        */
        if(rotationCount > 5)
        {
            rotationCount = 0;
            RobotRotationControl(540, 1, 600, 1);
            /*
            ** 脳陋脥盲露炉脳梅脥锚鲁脡拢卢脩掳脧脽脛拢脢陆脳陋脪媒碌录脛拢脢陆
            */
            runMode = LEAD;
        }
        //wait_for_timeout(100);
    }
    else
    {
        rotationCount = 0;
    }
}

uint8_t stop_count = 0;
uint8_t delay_flag = 0;
/*
 * 脪媒碌录脠脦脦帽
 */
void LeadFunc(void)
{
    uint8_t robotPos;
    int8_t posErr;


//    /* 禄帽脠隆碌卤脟掳碌脛脪媒碌录脧脽脦禄脰脙脨脜脧垄 */
//    leadLinePosi.poFL = CANRxData[CAMERA_INDEX_FRONT_LEFT].rxdata[0];
//    leadLinePosi2.poFL = CANRxData[CAMERA_INDEX_FRONT_LEFT].rxdata[1];
//    
//    robotPos = leadLinePosi.poFL;// + leadLinePosi2.poFL) / 2;
//    posErr = leadLinePosi.poFL - leadLinePosi2.poFL;
    
//    if (robotPos >= 100)//右转
//    {
//        rightMotorSpeed.rpm = 300;
//        leftMotorSpeed.rpm = 0;
//    }
//    else if(robotPos <= 60)//左转
//    {
//        rightMotorSpeed.rpm = 0;
//        leftMotorSpeed.rpm = 300;
//    }
//    else
//    {
//        rightMotorSpeed.rpm = 300;
//        leftMotorSpeed.rpm = 300;
//    }
    if(delay_flag == 0)
    {
        rightMotorSpeed.rpm = 1000;
        leftMotorSpeed.rpm = 1000;
        MotorDriverTask();
        wait_for_timeout(600);
        delay_flag = 1;
    }
    else
    {
        SearchFlowerFunc();
    }
//    if(leadLinePosi.poFL == 0 && leadLinePosi2.poFL == 0)
//    {
//        stop_count ++;
//    }
//    else
//    {
//        stop_count = 0;
//    }
//    if(stop_count > 5)
//    {
//        runMode = PLACE;
//        rightMotorSpeed.rpm = 0;
//        leftMotorSpeed.rpm = 0;
//        MotorDriverTask();
//        stop_count = 0;
//        //for(;;);
//    }


}

/*
 * 掳脷禄篓脠脦脦帽
 */
void PlaceFunc(void)
{
    uint8_t firstPos_Volt;  

    //Init position
    //1.举升电机回到初始位置
    if(0!=upmotor_firstPosition_Set()){
        //while(1){
            wait_for_timeout(500);
            firstPos_Volt = GPIO_ReadInputDataBit(JOY_A_KEY_Port,JOY_A_KEY_Pin);
            //usart1Printf("upmotor position set error, please reset~%d", firstPos_Volt);
        //}
    }
    //usart1Printf("upmotor position set ok, please reset~");
    wait_for_timeout(100);
    
    //夹持电机落下
    if(1==Ingrap_Action_down(320)){
        for(;;){
            wait_for_timeout(100);
            //usart1Printf("upmotor down set ok, please reset~");
        }
    }
    /*grapmotor setting, first position find*/
    //usart1Printf("grapmotor first position find~");
    wait_for_timeout(100);
    
    //3.夹持电机回到初始位置
    if(0!=grapmotor_firstPosition_Set()){
        //while(1){
            wait_for_timeout(50);
            firstPos_Volt = GPIO_ReadInputDataBit(JOY_A_KEY_Port,JOY_A_KEY_Pin);
            //usart1Printf("grapmotor position set error, please reset%d~",firstPos_Volt);
        //}
    }
    wait_for_timeout(100);
    
    leftMotorSpeed.dir = 0;
    rightMotorSpeed.dir = 0;
    leftMotorSpeed.rpm = 1000;
    rightMotorSpeed.rpm = 800;
    MotorDriverTask();
    wait_for_timeout(50);
    leftMotorSpeed.rpm = 0;
    rightMotorSpeed.rpm = 0;
    MotorDriverTask();
    //4.夹持电机升起
    if(1==Ingrap_Action_Up(300)){
        for(;;){
            wait_for_timeout(100);
            //usart1Printf("upmotor up set ok, please reset~");
        }
    }
    
    RobotRotationControl(1000, 1, 600, 1);
    leftMotorSpeed.dir = 1;
    rightMotorSpeed.dir = 1;
    leftMotorSpeed.rpm = 1000;
    rightMotorSpeed.rpm = 1000;
    MotorDriverTask();
    wait_for_timeout(800);
    /* go to idle mode, end */
    runMode = IDLE;
}
/*
** 
*/
void IdleFunc(void)
{
    leftMotorSpeed.dir = 1;
    rightMotorSpeed.dir = 1;
    leftMotorSpeed.rpm = 0;
    rightMotorSpeed.rpm = 0;
    MotorDriverTask();
}


/*
** 脡脧虏茫驴脴脰脝脗脽录颅
*/
uint8_t RunInModeTask(void)
{
    switch(runMode)
    {
        case SEARCHFLOWER:  /* 脩掳禄篓 */
            SearchFlowerFunc();
        break;
        case CARRY:         /* 掳谩禄篓 */
            CarryFlowerFunc();
        break;
        case SEARCHLINE:    /* 脩掳脧脽 */
            SearchLineFunc();
        break;
        case LEAD:          /* 脪媒碌录 */
            LeadFunc();
        break;
        case PLACE:         /* 路脜禄篓 */
            PlaceFunc();
        break;
        case IDLE:          /* 驴脮脧脨 */
            IdleFunc();
        break;
    }
    return 0;
}


/*
* @func SendLineLocationCAN1
*
* @param[in] none
* 
* @return none
*
* @brief 
*/
uint8_t MainInfoSendTask(void)
{
    mainInfo[0].txdata[0] = runMode; //脭脣脨脨脛拢脢陆
    
    Can1WriteData(mainInfo[0].ID, 
                  mainInfo[0].isextend,
                  mainInfo[0].isdataframe, 
                  mainInfo[0].length, 
                  (uint8_t *)(&mainInfo[0].txdata));
    return 0;
}

/*
** 脢卤录盲脝卢脗脰脩炉路篓拢卢脠脦脦帽麓娄脌铆潞炉脢媒
*/
void TimerTaskProcess(void)
{
	uint8_t i, len;
	if(Task_Queue.AllEnable > 0)
    {
		if(Task_Queue.size > 0)
        {
			len = Task_Queue.size;
			for(i=0; i<len; i++)
            {
				if(Task_Queue.Task[i].Stutas == 1)
                {
                    Task_Queue.Task[i].TimeoutTask();
                    Task_Queue.Task[i].Stutas = 0;
				}
			}
		}
    }
}

